/*
  File:    DiffusionSolverSteadyState.cpp
  Purpose: Provides a fully nonlinear diffusional solver 
           for steady state problems, using our brand new ideas. 
		  In particular, it solves the problem 
		   
		  0 = div( DiffusionConstant(u,x,y) . grad(u) ) + Sources(u,x,y)
		   
		  with boundary conditions on the left, right, bottom, 
		  and top, of type:
		    
		  'D': Dirichlet
		  'N': Neuman
		  'E': Extrapolation/continuity

           Assumed:
           Can evaluate DiffusionConstant, Source, and 
		  boundary conditions at any point (x,y) where 
		  appropriate. 
    
           DiffusionConstant and Source can be nonlinear 
		  in u. 
   
           Result: 
           The solution u(x,y,t) at t+dt.   
		   
  Contact: Paul Macklin
           pmacklin@math.uci.edu
		  http://math.uci.edu/~pmacklin
*/

#include <iostream>
#include <cstdlib>
#include <cstdio>
#include <cmath>

using namespace std;

#include "../Matrix.h"
#include "../ImprovedMath.h"

/*
extern int Xnodes;
extern int Ynodes;
extern Matrix Phi;
*/

#include "../stopwatch.h"

class SolverNode{
public:
 int i;
 int j;
 SolverNode* pNextSolverNode;
 SolverNode();
};

SolverNode::SolverNode()
{
 i=0;
 j=0;
 pNextSolverNode = NULL;
}

double ImplicitUpdate(
Matrix& Solution, 
double (*Diffusion)(double,double,double,int,int),
double (*SourceTimesU)(double,double,double,int,int),
double (*SourceRemainder)(double,double,double,int,int), 
int i, int j, 
double x, double y, double dx, double dy )
{
 double dx2 = square(dx);
 double dy2 = square(dy);
 
 double Uij = *Solution(i,j);
 double Uim1_j = *Solution(i-1,j);
 double Uip1_j = *Solution(i+1,j);
 double Ui_jm1 = *Solution(i,j-1);
 double Ui_jp1 = *Solution(i,j+1);
 
 double dt = 1.0;
/* 
 double CoeffXell = Diffusion(0.5*(Uim1_j+Uij),x-0.5*dx,y);
 double CoeffXr   = Diffusion(0.5*(Uip1_j+Uij),x+0.5*dx,y);
 double CoeffYell = Diffusion(0.5*(Ui_jm1+Uij),x,y-0.5*dy);
 double CoeffYr   = Diffusion(0.5*(Ui_jp1+Uij),x,y+0.5*dy);
*/
 
 double CenterDiffusion = Diffusion(Uij,x,y,i,j);

 double CoeffXell = 0.5*( Diffusion(Uim1_j,x-dx,y,i-1,j) + CenterDiffusion );
 double CoeffXr   = 0.5*( Diffusion(Uip1_j,x+dx,y,i+1,j) + CenterDiffusion );
 double CoeffYell = 0.5*( Diffusion(Ui_jm1,x,y-dy,i,j-1) + CenterDiffusion );
 double CoeffYr   = 0.5*( Diffusion(Ui_jp1,x,y+dy,i,j+1) + CenterDiffusion );
 
 double SrcLin = SourceTimesU(Uij,x,y,i,j);
 double SrcNonLin = SourceRemainder(Uij,x,y,i,j);
 
 return ( Uij/dt + (CoeffXell*Uim1_j + CoeffXr*Uip1_j)/dx2 
              + (CoeffYell*Ui_jm1 + CoeffYr*Ui_jp1)/dy2 + SrcNonLin )
       /( 1.0/dt + (CoeffXell+CoeffXr)/dx2 + (CoeffYell+CoeffYr)/dy2 - SrcLin );
}
/*
double ImplicitConvectUpdate(
					  Matrix& Solution, 
					  double (*Diffusion)(double,double,double,int,int),
					  double (*SourceTimesU)(double,double,double,int,int),
					  double (*SourceRemainder)(double,double,double,int,int), 
					  int i, int j, 
					  double x, double y, double dx, double dy )
{
	double dx2 = square(dx);
	double dy2 = square(dy);
	
	double Uij = *Solution(i,j);
	double Uim1_j = *Solution(i-1,j);
	double Uip1_j = *Solution(i+1,j);
	double Ui_jm1 = *Solution(i,j-1);
	double Ui_jp1 = *Solution(i,j+1);
	
	double dt = 1.0;
	
	
	double CenterDiffusion = Diffusion(Uij,x,y,i,j);
	
	double CoeffXell = 0.5*( Diffusion(Uim1_j,x-dx,y,i-1,j) + CenterDiffusion );
	double CoeffXr   = 0.5*( Diffusion(Uip1_j,x+dx,y,i+1,j) + CenterDiffusion );
	double CoeffYell = 0.5*( Diffusion(Ui_jm1,x,y-dy,i,j-1) + CenterDiffusion );
	double CoeffYr   = 0.5*( Diffusion(Ui_jp1,x,y+dy,i,j+1) + CenterDiffusion );
	
	double SrcLin = SourceTimesU(Uij,x,y,i,j);
	double SrcNonLin = SourceRemainder(Uij,x,y,i,j);
	
	return ( Uij/dt + (CoeffXell*Uim1_j + CoeffXr*Uip1_j)/dx2 
			+ (CoeffYell*Ui_jm1 + CoeffYr*Ui_jp1)/dy2 + SrcNonLin )
	/( 1.0/dt + (CoeffXell+CoeffXr)/dx2 + (CoeffYell+CoeffYr)/dy2 - SrcLin );
}
*/

double UpdateBoundaryPoint(Matrix& Solution, 
double (*Diffusion)(double,double,double,int,int),
double (*SourceTimesU)(double,double,double,int,int),
double (*SourceRemainder)(double,double,double,int,int), 
int i, int j, 
double x, double y, double dx, double dy)
{

double dx2 = square(dx);
double dy2 = square(dy);
double dt = 1.0;
double Uij = *Solution(i,j);
double CenterDiffusion = Diffusion(Uij,x,y,i,j);
 double Uim1_j = 0;
 double Uip1_j = 0;
 double Ui_jm1 = 0;
 double Ui_jp1 = 0;


 double CoeffXell = 0;
 double CoeffXr   = 0;
 double CoeffYell = 0;
 double CoeffYr   = 0;
 if(i==0)
 {
 if(j == 0)
 {

  Uim1_j = *Solution(i+1,j);
  Uip1_j = *Solution(i+1,j);
  Ui_jm1 = *Solution(i,j+1);
  Ui_jp1 = *Solution(i,j+1);


  CoeffXell = 0.5*( Diffusion(Uim1_j,x+dx,y,i+1,j) + CenterDiffusion );
  CoeffXr   = 0.5*( Diffusion(Uip1_j,x+dx,y,i+1,j) + CenterDiffusion );
  CoeffYell = 0.5*( Diffusion(Ui_jm1,x,y+dy,i,j+1) + CenterDiffusion );
  CoeffYr   = 0.5*( Diffusion(Ui_jp1,x,y+dy,i,j+1) + CenterDiffusion );

 
 }
 else if(j == Solution.TellCols() - 1)
 {
  Uim1_j = *Solution(i+1,j);
  Uip1_j = *Solution(i+1,j);
  Ui_jm1 = *Solution(i,j-1);
  Ui_jp1 = *Solution(i,j-1);


  CoeffXell = 0.5*( Diffusion(Uim1_j,x+dx,y,i+1,j) + CenterDiffusion );
  CoeffXr   = 0.5*( Diffusion(Uip1_j,x+dx,y,i+1,j) + CenterDiffusion );
  CoeffYell = 0.5*( Diffusion(Ui_jm1,x,y-dy,i,j-1) + CenterDiffusion );
  CoeffYr   = 0.5*( Diffusion(Ui_jp1,x,y-dy,i,j-1) + CenterDiffusion );

 
 }
 else
 {
 
  Uim1_j = *Solution(i+1,j);
  Uip1_j = *Solution(i+1,j);
  Ui_jm1 = *Solution(i,j-1);
  Ui_jp1 = *Solution(i,j+1);


  CoeffXell = 0.5*( Diffusion(Uim1_j,x+dx,y,i+1,j) + CenterDiffusion );
  CoeffXr   = 0.5*( Diffusion(Uip1_j,x+dx,y,i+1,j) + CenterDiffusion );
  CoeffYell = 0.5*( Diffusion(Ui_jm1,x,y-dy,i,j-1) + CenterDiffusion );
  CoeffYr   = 0.5*( Diffusion(Ui_jp1,x,y+dy,i,j+1) + CenterDiffusion );

 
 }
 }
 
 if(i==Solution.TellRows() - 1)
 {
 if(j == 0)
 {
  Uim1_j = *Solution(i-1,j);
  Uip1_j = *Solution(i-1,j);
  Ui_jm1 = *Solution(i,j+1);
  Ui_jp1 = *Solution(i,j+1);
 
  CoeffXell = 0.5*( Diffusion(Uim1_j,x-dx,y,i-1,j) + CenterDiffusion );
  CoeffXr   = 0.5*( Diffusion(Uip1_j,x-dx,y,i-1,j) + CenterDiffusion );
  CoeffYell = 0.5*( Diffusion(Ui_jm1,x,y+dy,i,j+1) + CenterDiffusion );
  CoeffYr   = 0.5*( Diffusion(Ui_jp1,x,y+dy,i,j+1) + CenterDiffusion );

 }
 else if(j == Solution.TellCols() - 1)
 {
  Uim1_j = *Solution(i-1,j);
  Uip1_j = *Solution(i-1,j);
  Ui_jm1 = *Solution(i,j-1);
  Ui_jp1 = *Solution(i,j-1);
 
  CoeffXell = 0.5*( Diffusion(Uim1_j,x-dx,y,i-1,j) + CenterDiffusion );
  CoeffXr   = 0.5*( Diffusion(Uip1_j,x-dx,y,i-1,j) + CenterDiffusion );
  CoeffYell = 0.5*( Diffusion(Ui_jm1,x,y-dy,i,j-1) + CenterDiffusion );
  CoeffYr   = 0.5*( Diffusion(Ui_jp1,x,y-dy,i,j-1) + CenterDiffusion );

 }
 else
 {
  Uim1_j = *Solution(i-1,j);
  Uip1_j = *Solution(i-1,j);
  Ui_jm1 = *Solution(i,j-1);
  Ui_jp1 = *Solution(i,j+1);
 
  CoeffXell = 0.5*( Diffusion(Uim1_j,x-dx,y,i-1,j) + CenterDiffusion );
  CoeffXr   = 0.5*( Diffusion(Uip1_j,x-dx,y,i-1,j) + CenterDiffusion );
  CoeffYell = 0.5*( Diffusion(Ui_jm1,x,y-dy,i,j-1) + CenterDiffusion );
  CoeffYr   = 0.5*( Diffusion(Ui_jp1,x,y+dy,i,j+1) + CenterDiffusion );

 }
 }
 
 
 if(j==0)
 {
 if(i == 0)
 {
 //done
  Uim1_j = *Solution(i+1,j);
  Uip1_j = *Solution(i+1,j);
  Ui_jm1 = *Solution(i,j+1);
  Ui_jp1 = *Solution(i,j+1);


  CoeffXell = 0.5*( Diffusion(Uim1_j,x+dx,y,i+1,j) + CenterDiffusion );
  CoeffXr   = 0.5*( Diffusion(Uip1_j,x+dx,y,i+1,j) + CenterDiffusion );
  CoeffYell = 0.5*( Diffusion(Ui_jm1,x,y+dy,i,j+1) + CenterDiffusion );
  CoeffYr   = 0.5*( Diffusion(Ui_jp1,x,y+dy,i,j+1) + CenterDiffusion );

 }
 else if(i == Solution.TellRows() - 1)
 {
 //done
  Uim1_j = *Solution(i-1,j);
  Uip1_j = *Solution(i-1,j);
  Ui_jm1 = *Solution(i,j+1);
  Ui_jp1 = *Solution(i,j+1);
 
  CoeffXell = 0.5*( Diffusion(Uim1_j,x-dx,y,i-1,j) + CenterDiffusion );
  CoeffXr   = 0.5*( Diffusion(Uip1_j,x-dx,y,i-1,j) + CenterDiffusion );
  CoeffYell = 0.5*( Diffusion(Ui_jm1,x,y+dy,i,j+1) + CenterDiffusion );
  CoeffYr   = 0.5*( Diffusion(Ui_jp1,x,y+dy,i,j+1) + CenterDiffusion );

 }
 else
 {
  Uim1_j = *Solution(i-1,j);
  Uip1_j = *Solution(i+1,j);
  Ui_jm1 = *Solution(i,j+1);
  Ui_jp1 = *Solution(i,j+1);
 
  CoeffXell = 0.5*( Diffusion(Uim1_j,x-dx,y,i-1,j) + CenterDiffusion );
  CoeffXr   = 0.5*( Diffusion(Uip1_j,x+dx,y,i+1,j) + CenterDiffusion );
  CoeffYell = 0.5*( Diffusion(Ui_jm1,x,y+dy,i,j+1) + CenterDiffusion );
  CoeffYr   = 0.5*( Diffusion(Ui_jp1,x,y+dy,i,j+1) + CenterDiffusion );

 }
 }
 
  if(j==Solution.TellCols() - 1)
 {
 if(i == 0)
 {
 //done
  Uim1_j = *Solution(i+1,j);
  Uip1_j = *Solution(i+1,j);
  Ui_jm1 = *Solution(i,j-1);
  Ui_jp1 = *Solution(i,j-1);


  CoeffXell = 0.5*( Diffusion(Uim1_j,x+dx,y,i+1,j) + CenterDiffusion );
  CoeffXr   = 0.5*( Diffusion(Uip1_j,x+dx,y,i+1,j) + CenterDiffusion );
  CoeffYell = 0.5*( Diffusion(Ui_jm1,x,y-dy,i,j-1) + CenterDiffusion );
  CoeffYr   = 0.5*( Diffusion(Ui_jp1,x,y-dy,i,j-1) + CenterDiffusion );
 }
 else if(i == Solution.TellRows() - 1)
 {
 //done
  Uim1_j = *Solution(i-1,j);
  Uip1_j = *Solution(i-1,j);
  Ui_jm1 = *Solution(i,j-1);
  Ui_jp1 = *Solution(i,j-1);
 
  CoeffXell = 0.5*( Diffusion(Uim1_j,x-dx,y,i-1,j) + CenterDiffusion );
  CoeffXr   = 0.5*( Diffusion(Uip1_j,x-dx,y,i-1,j) + CenterDiffusion );
  CoeffYell = 0.5*( Diffusion(Ui_jm1,x,y-dy,i,j-1) + CenterDiffusion );
  CoeffYr   = 0.5*( Diffusion(Ui_jp1,x,y-dy,i,j-1) + CenterDiffusion );
 }
 else
 {
  Uim1_j = *Solution(i-1,j);
  Uip1_j = *Solution(i+1,j);
  Ui_jm1 = *Solution(i,j-1);
  Ui_jp1 = *Solution(i,j-1);
 
  CoeffXell = 0.5*( Diffusion(Uim1_j,x-dx,y,i-1,j) + CenterDiffusion );
  CoeffXr   = 0.5*( Diffusion(Uip1_j,x+dx,y,i+1,j) + CenterDiffusion );
  CoeffYell = 0.5*( Diffusion(Ui_jm1,x,y-dy,i,j-1) + CenterDiffusion );
  CoeffYr   = 0.5*( Diffusion(Ui_jp1,x,y-dy,i,j-1) + CenterDiffusion );

 }
 }

 

 
 
 double SrcLin = SourceTimesU(Uij,x,y,i,j);
 double SrcNonLin = SourceRemainder(Uij,x,y,i,j);
 
 return ( Uij/dt + (CoeffXell*Uim1_j + CoeffXr*Uip1_j)/dx2 
              + (CoeffYell*Ui_jm1 + CoeffYr*Ui_jp1)/dy2 + SrcNonLin )
       /( 1.0/dt + (CoeffXell+CoeffXr)/dx2 + (CoeffYell+CoeffYr)/dy2 - SrcLin );


}



double UpdateBoundaryPoint( Matrix& Solution, 
   double (*LeftBC)(double),   char LeftBCtype,  
   double (*RightBC)(double),  char RightBCtype,     
   double (*BottomBC)(double), char BottomBCtype,   
   double (*TopBC)(double),    char TopBCtype,  
   int i, int j, 
   double x, double y )
{
 if( i == 0 )
 {  
  if( LeftBCtype == 'd' || LeftBCtype == 'D' )
  { return LeftBC(y); }
  if( LeftBCtype == 'n' || LeftBCtype == 'N' )
  { return (4* *Solution(1,j) - *Solution(2,j) - 2*LeftBC(y) )/3.0; }  
  if( LeftBCtype == 'e' || LeftBCtype == 'E' )
  { return 2.0*(*Solution(1,j))-1.0*(*Solution(2,j)); }  
//  if( LeftBCtype == 'e' || LeftBCtype == 'E' )
//  { return 3.0*(*Solution(1,j)-*Solution(2,j)) + *Solution(3,j); }
  if( LeftBCtype == 'e' || LeftBCtype == 'E' )
  {
   return  4.0*(*Solution(1,j))-6.0*(*Solution(2,j))
          +4.0*(*Solution(3,j))-1.0*(*Solution(4,j));
  }
  return 0.0;   
 }
 
 if( i == Solution.TellRows() - 1)
 {
  if( RightBCtype == 'd' || RightBCtype == 'D' )
  { return RightBC(y); }
  if( RightBCtype == 'n' || RightBCtype == 'N' )
  { return (2*RightBC(y) + 4* *Solution(i-1,j) - *Solution(i-2,j) )/3.0; }  
  if( RightBCtype == 'e' || RightBCtype == 'E' )
  { return 2.0*(*Solution(i-1,j))-1.0*(*Solution(i-2,j)); }  
//  if( RightBCtype == 'e' || RightBCtype == 'E' )
//  { return 3.0*(*Solution(i-1,j)-*Solution(i-2,j)) + *Solution(i-3,j); }
  if( RightBCtype == 'e' || RightBCtype == 'E' )
  {
   return  4.0*(*Solution(i-1,j))-6.0*(*Solution(i-2,j))
          +4.0*(*Solution(i-3,j))-1.0*(*Solution(i-4,j));
  }
  return 0.0;   
 }

 if( j == 0 )
 {  
  if( BottomBCtype == 'd' || BottomBCtype == 'D' )
  { return BottomBC(x); }
  if( BottomBCtype == 'n' || BottomBCtype == 'N' )
  { return (4* *Solution(i,1) - *Solution(i,2) - 2*BottomBC(x) )/3.0; }  
  if( BottomBCtype == 'e' || BottomBCtype == 'E' )
  { return 2.0*(*Solution(i,1))-1.0*(*Solution(i,2)); }
//  if( BottomBCtype == 'e' || BottomBCtype == 'E' )
//  { return 3.0*(*Solution(i,1)-*Solution(i,2)) + *Solution(i,3); }
  if( BottomBCtype == 'e' || BottomBCtype == 'E' )
  {
   return  4.0*(*Solution(i,1))-6.0*(*Solution(i,2))
          +4.0*(*Solution(i,3))-1.0*(*Solution(i,4));
  }
  return 0.0;   
 }
 
 if( j == Solution.TellCols() - 1)
 {
  if( TopBCtype == 'd' || TopBCtype == 'D' )
  { return TopBC(x); }
  if( TopBCtype == 'n' || TopBCtype == 'N' )
  { return (2*TopBC(x) + 4* *Solution(i,j-1) - *Solution(i,j-2) )/3.0; }  
  if( TopBCtype == 'e' || TopBCtype == 'E' )
  { return 2.0*(*Solution(i,j-1))-1.0*(*Solution(i,j-2)); }
//  if( TopBCtype == 'e' || TopBCtype == 'E' )
//  { return 3.0*(*Solution(i,j-1)-*Solution(i,j-2)) + *Solution(i,j-3); }
  if( TopBCtype == 'e' || TopBCtype == 'E' )
  {
   return  4.0*(*Solution(i,j-1))-6.0*(*Solution(i,j-2))
          +4.0*(*Solution(i,j-3))-1.0*(*Solution(i,j-4));
  }
  return 0.0;   
 }
 return 0.0;
}

Matrix NonlinearDiffusionSteadyState( 
   Matrix& X, Matrix& Y, 
   Matrix& InitialGuess ,
   double (*LeftBC)(double),   char LeftBCtype,  
   double (*RightBC)(double),  char RightBCtype,     
   double (*BottomBC)(double), char BottomBCtype,   
   double (*TopBC)(double),    char TopBCtype,  
   double (*DiffusionConstant)(double,double,double,int,int), 
   double (*SourceTimesU)(double,double,double,int,int),
   double (*SourceRemainder)(double,double,double,int,int),
   int MaxIterations, double Tolerance )
{
 Matrix Output = InitialGuess; 

 Matrix Helper = zeros( Output.TellRows() , Output.TellCols() );
 
 int Iteration = 0;
 double Error  = 1e10;
 
 double dx = *X(1)-*X(0);
 double dy = *Y(1)-*Y(0);
 
 int i,j;
 
 SolverNode* pHeadSolverNode = NULL;
 SolverNode* pCurrentSolverNode = pHeadSolverNode;

 double SelectionCriterion = 0.1 * Error; 
 double SelectionRatio = 0.25;
 
 bool GiveUp = false;
// int GiveUpThreshold = (int) ( 0.0005 * Xnodes * Ynodes );
 int GiveUpThreshold = (int) floor( (0.015*X.TellCols())*(0.015*Y.TellCols()) );
 
 
 while( Error > Tolerance && Iteration < MaxIterations && !GiveUp )
 {
  Error = 0;

  // update the boundaries
  
  
  for( i=0 ; i < Output.TellRows() ; i++ )
  {
   j = 0;
   double NewValue = UpdateBoundaryPoint(Output,
                                          LeftBC,LeftBCtype, 
		               				      RightBC,RightBCtype,
	 									  BottomBC,BottomBCtype,
	 									  TopBC,TopBCtype,
   	 									  i,j,*X(i),*Y(j));
   double change = fabs( NewValue - *Output(i,j) );
   if( change > Error )
   { Error = change; }
   *Output(i,j) = NewValue; 
   j = Output.TellCols()-1;
   NewValue = UpdateBoundaryPoint(Output,
                                   LeftBC,LeftBCtype, 
							       RightBC,RightBCtype,
								   BottomBC,BottomBCtype,
								   TopBC,TopBCtype,
   								   i,j,*X(i),*Y(j));
   change = fabs( NewValue - *Output(i,j) );
   if( change > Error )
   { Error = change; }
   *Output(i,j) = NewValue; 
  }
   
 
  for( j=1 ; j < Output.TellCols()-1 ; j++ )
  {
   i = 0;
   double NewValue = UpdateBoundaryPoint(Output,
                                         LeftBC,LeftBCtype, 
                                         RightBC,RightBCtype,
										 BottomBC,BottomBCtype,
										 TopBC,TopBCtype,
   										 i,j,*X(i),*Y(j));
   double change = fabs( NewValue - *Output(i,j) );
   if( change > Error )
   { Error = change; }
   *Output(i,j) = NewValue; 
   i = Output.TellRows()-1;
   NewValue = UpdateBoundaryPoint(Output,
                                  LeftBC,LeftBCtype, 
								  RightBC,RightBCtype,
								  BottomBC,BottomBCtype,
								  TopBC,TopBCtype,
   								  i,j,*X(i),*Y(j));
   change = fabs( NewValue - *Output(i,j) );
   if( change > Error )
   { Error = change; }
   *Output(i,j) = NewValue; 
  }
   
  // Gauss-Siedel stype updates in alternating sweep directions
  // adaptivity: iterate fully, then iterate fully and flag points, 
  //             then only update flagged points (2x)
  
  if( Iteration % 4 == 0 )
  {
//cout<<Iteration<<endl;
   for( j=1 ; j < Output.TellCols()-1 ; j++ )
   {
    for( i=1 ; i < Output.TellRows()-1 ; i++ )
    {
	 
     double NewValue = ImplicitUpdate( Output, 
                                       DiffusionConstant, SourceTimesU, SourceRemainder, 
                                       i,j,*X(i),*Y(j),dx,dy );
	 
     double change = fabs( NewValue - *Output(i,j) );
     if( change > Error )
     { Error = change; } 
     *Output(i,j) = NewValue; 
	  
     if( Iteration % 2 == 1 && change > SelectionCriterion )
	 {
	  SolverNode* pNewNode;
	  pNewNode = new SolverNode;
	  pNewNode->i = i;
	  pNewNode->j = j;
	  if( pCurrentSolverNode )
	  {
	   pCurrentSolverNode->pNextSolverNode = pNewNode; 
	   pCurrentSolverNode = pNewNode;
      }	  
	  else
	  {
	   pHeadSolverNode = pNewNode;
	   pCurrentSolverNode = pNewNode;
	  }
	 }
	 
    }
   }   
   
  }
  
  if( Iteration % 4 == 1 )
  {
 //cout<<Iteration<<endl;

   for( j=Output.TellCols()-2 ; j > 0 ; j-- )
   {
    for( i=1 ; i < Output.TellRows()-1 ; i++ )
    {
	 
     double NewValue = ImplicitUpdate( Output, 
                                       DiffusionConstant, SourceTimesU, SourceRemainder, 
                                       i,j,*X(i),*Y(j),dx,dy );
     double change = fabs( NewValue - *Output(i,j) );
     if( change > Error )
     { Error = change; } 
     *Output(i,j) = NewValue; 
	 
     if( Iteration % 2 == 1 && change > SelectionCriterion )
	 {
	  SolverNode* pNewNode;
	  pNewNode = new SolverNode;
	  pNewNode->i = i;
	  pNewNode->j = j;
	  if( pCurrentSolverNode )
	  {
	   pCurrentSolverNode->pNextSolverNode = pNewNode; 
	   pCurrentSolverNode = pNewNode;
      }	  
	  else
	  {
	   pHeadSolverNode = pNewNode;
	   pCurrentSolverNode = pNewNode;
	  }
	 }
	 
    }
   }
  }
  
  if( Iteration % 4 == 2 )
  {
//cout<<Iteration<<endl;

   for( j=Output.TellCols()-2 ; j > 0 ; j-- )
   {
    for( i=Output.TellRows()-2 ; i > 0 ; i-- )
    {
     double NewValue = ImplicitUpdate( Output, 
                                       DiffusionConstant, SourceTimesU, SourceRemainder, 
                                       i,j,*X(i),*Y(j),dx,dy );
     double change = fabs( NewValue - *Output(i,j) );
     if( change > Error )
     { Error = change; } 
     *Output(i,j) = NewValue; 
	 
     if( Iteration % 2 == 1 && change > SelectionCriterion )
	 {
	  SolverNode* pNewNode;
	  pNewNode = new SolverNode;
	  pNewNode->i = i;
	  pNewNode->j = j;
	  if( pCurrentSolverNode )
	  {
	   pCurrentSolverNode->pNextSolverNode = pNewNode; 
	   pCurrentSolverNode = pNewNode;
      }	  
	  else
	  {
	   pHeadSolverNode = pNewNode;
	   pCurrentSolverNode = pNewNode;
	  }
	 }
	 
    }
   }
  }
  
  if( Iteration % 4 == 3 )
  {
//cout<<Iteration<<endl;

   for( j=1 ; j < Output.TellCols()-1 ; j++ )
   {
    for( i=Output.TellRows()-2 ; i > 0 ; i-- )
    {
	
	
     double NewValue = ImplicitUpdate( Output, 
                                       DiffusionConstant, SourceTimesU, SourceRemainder, 
                                       i,j,*X(i),*Y(j),dx,dy );
     double change = fabs( NewValue - *Output(i,j) );
     if( change > Error )
     { Error = change; } 
     *Output(i,j) = NewValue; 
	 
     if( Iteration % 2 == 1 && change > SelectionCriterion )
	 {
	  SolverNode* pNewNode;
	  pNewNode = new SolverNode;
	  pNewNode->i = i;
	  pNewNode->j = j;
	  if( pCurrentSolverNode )
	  {
	   pCurrentSolverNode->pNextSolverNode = pNewNode; 
	   pCurrentSolverNode = pNewNode;
      }	  
	  else
	  {
	   pHeadSolverNode = pNewNode;
	   pCurrentSolverNode = pNewNode;
	  }
	 }
	 
    }
   }
  }
  
  // Set the selection criterion if it's an even run
  
  if( Iteration % 2 == 0 )
  { SelectionCriterion = SelectionRatio * Error; }
  
  // if it's an odd iteration, then we have a list of points to 
  // focus on. Run through the flagged nodes twice, and then delete them. 
  // These runs don't count towards the iteration count!!
  
  if( Iteration % 2 == 1 )
  {
//cout<<Iteration<<"begin"<<endl;

   pCurrentSolverNode = pHeadSolverNode; 
   while( pCurrentSolverNode )
   {     
    i = pCurrentSolverNode->i;
	j = pCurrentSolverNode->j;
    double NewValue = ImplicitUpdate( Output, 
                                      DiffusionConstant, SourceTimesU, SourceRemainder, 
                                      i,j,*X(i),*Y(j),dx,dy );
    *Output(i,j) = NewValue; 
	pCurrentSolverNode = pCurrentSolverNode->pNextSolverNode;
   }
   pCurrentSolverNode = pHeadSolverNode; 
   
   while( pCurrentSolverNode )
   {     
    i = pCurrentSolverNode->i;
	j = pCurrentSolverNode->j;
    double NewValue = ImplicitUpdate( Output, 
                                      DiffusionConstant, SourceTimesU, SourceRemainder, 
                                      i,j,*X(i),*Y(j),dx,dy );
    *Output(i,j) = NewValue; 
	pCurrentSolverNode = pCurrentSolverNode->pNextSolverNode;
	//cout<<Iteration<<"end"<<endl;

   }
   
   // delete the selected solver nodes; 
   pCurrentSolverNode = pHeadSolverNode;
   while( pCurrentSolverNode )
   {
    pCurrentSolverNode = pHeadSolverNode->pNextSolverNode;
    delete pHeadSolverNode; 
    pHeadSolverNode = pCurrentSolverNode;
   }
  }

  Iteration++;
  
  //Can get herecout<<"Can get here"<<endl;
 } 
 
 cout << "summary:: iterations: " << Iteration << endl
      << "          error     : " << Error << endl << endl;
 if( GiveUp )
 {
  cout << "Gave up because adaptivity was being focused on less than 0.0225% of nodes!" << endl
//       << "(" << NumberOfSolverPoints << " nodes)" 
	   << endl << endl;   
 } 
 return Output;
}
 
Matrix NonlinearDiffusionSteadyStateNonAdaptive( 
   Matrix& X, Matrix& Y, 
   Matrix& InitialGuess ,
   double (*LeftBC)(double),   char LeftBCtype,  
   double (*RightBC)(double),  char RightBCtype,     
   double (*BottomBC)(double), char BottomBCtype,   
   double (*TopBC)(double),    char TopBCtype,  
   double (*DiffusionConstant)(double,double,double,int,int), 
   double (*SourceTimesU)(double,double,double,int,int),
   double (*SourceRemainder)(double,double,double,int,int),
   int MaxIterations, double Tolerance )
{
 Matrix Output = InitialGuess; 
 
 int Iteration = 0;
 double Error  = 1e10;
 
 double dx = *X(1)-*X(0);
 double dy = *Y(1)-*Y(0);
 
 int i,j;
 
 SolverNode* pHeadSolverNode = NULL;
 SolverNode* pCurrentSolverNode = pHeadSolverNode;
  
 bool GiveUp = false;
// int GiveUpThreshold = (int) ( 0.0005 * Xnodes * Ynodes );
// int GiveUpThreshold = (int) floor( (0.015*X.TellCols())*(0.015*Y.TellCols()) );
 int GiveUpThreshold = 10;

 int Count = 0;

 while( Error > Tolerance && Iteration < MaxIterations && !GiveUp )
 {
  Error = 0;
  Count = 0;

  // update the boundaries
  
  for( i=0 ; i < Output.TellRows() ; i++ )
  {
   j = 0;
   double NewValue = UpdateBoundaryPoint(Output,
                                          LeftBC,LeftBCtype, 
		               				      RightBC,RightBCtype,
	 									  BottomBC,BottomBCtype,
	 									  TopBC,TopBCtype,
   	 									  i,j,*X(i),*Y(j));
   double change = fabs( NewValue - *Output(i,j) );
   if( change > Error )
   { Error = change; }
   *Output(i,j) = NewValue; 
   j = Output.TellCols()-1;
   NewValue = UpdateBoundaryPoint(Output,
                                   LeftBC,LeftBCtype, 
							       RightBC,RightBCtype,
								   BottomBC,BottomBCtype,
								   TopBC,TopBCtype,
   								   i,j,*X(i),*Y(j));
   change = fabs( NewValue - *Output(i,j) );
   if( change > Error )
   { Error = change; }
   *Output(i,j) = NewValue; 
  }
 
  for( j=1 ; j < Output.TellCols()-1 ; j++ )
  {
   i = 0;
   double NewValue = UpdateBoundaryPoint(Output,
                                         LeftBC,LeftBCtype, 
                                         RightBC,RightBCtype,
										 BottomBC,BottomBCtype,
										 TopBC,TopBCtype,
   										 i,j,*X(i),*Y(j));
   double change = fabs( NewValue - *Output(i,j) );
   if( change > Error )
   { Error = change; }
   *Output(i,j) = NewValue; 
   i = Output.TellRows()-1;
   NewValue = UpdateBoundaryPoint(Output,
                                  LeftBC,LeftBCtype, 
								  RightBC,RightBCtype,
								  BottomBC,BottomBCtype,
								  TopBC,TopBCtype,
   								  i,j,*X(i),*Y(j));
   change = fabs( NewValue - *Output(i,j) );
   if( change > Error )
   { Error = change; }
   *Output(i,j) = NewValue; 
  }
  
  // Gauss-Siedel stype updates in alternating sweep directions
  
  if( Iteration % 4 == 0 )
  {
   for( j=1 ; j < Output.TellCols()-1 ; j++ )
   {
    for( i=1 ; i < Output.TellRows()-1 ; i++ )
    {
     double NewValue = ImplicitUpdate( Output, 
                                       DiffusionConstant, SourceTimesU, SourceRemainder, 
                                       i,j,*X(i),*Y(j),dx,dy );
     double change = fabs( NewValue - *Output(i,j) );
     if( change > Error )
     { Error = change; } 
     
     if( change > 0.75*Tolerance )
     { Count++; }
     
     *Output(i,j) = NewValue; 
    }
   }
  }
  
  if( Iteration % 4 == 1 )
  {
   for( j=Output.TellCols()-2 ; j > 0 ; j-- )
   {
    for( i=1 ; i < Output.TellRows()-1 ; i++ )
    {
     double NewValue = ImplicitUpdate( Output, 
                                       DiffusionConstant, SourceTimesU, SourceRemainder, 
                                       i,j,*X(i),*Y(j),dx,dy );
     double change = fabs( NewValue - *Output(i,j) );
     if( change > Error )
     { Error = change; } 
     
     if( change > 0.75*Tolerance )
     { Count++; }
     
     *Output(i,j) = NewValue; 
    }
   }
  }
  
  if( Iteration % 4 == 2 )
  {
   for( j=Output.TellCols()-2 ; j > 0 ; j-- )
   {
    for( i=Output.TellRows()-2 ; i > 0 ; i-- )
    {
     double NewValue = ImplicitUpdate( Output, 
                                       DiffusionConstant, SourceTimesU, SourceRemainder, 
                                       i,j,*X(i),*Y(j),dx,dy );
     double change = fabs( NewValue - *Output(i,j) );
     if( change > Error )
     { Error = change; } 
     
     if( change > 0.75*Tolerance )
     { Count++; }
     
     *Output(i,j) = NewValue; 
    }
   }
  }
  
  if( Iteration % 4 == 3 )
  {
   for( j=1 ; j < Output.TellCols()-1 ; j++ )
   {
    for( i=Output.TellRows()-2 ; i > 0 ; i-- )
    {
     double NewValue = ImplicitUpdate( Output, 
                                       DiffusionConstant, SourceTimesU, SourceRemainder, 
                                       i,j,*X(i),*Y(j),dx,dy );
     double change = fabs( NewValue - *Output(i,j) );
     if( change > Error )
     { Error = change; } 

     if( change > 0.75*Tolerance )
     { Count++; }

     *Output(i,j) = NewValue; 
    }
   }
  }
  
  if( Count < GiveUpThreshold )
  { GiveUp = true; }

  Iteration++;
 } 
 cout << "summary:: iterations: " << Iteration << endl
      << "          error     : " << Error << endl << endl;
 if( GiveUp )
 {
  cout << "Gave up because effort was being focused on less than 0.0225% of nodes!" << endl 
       << "(" << Count << " nodes)" << endl << endl; 
 } 

 return Output;
}


